
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       ErrorMode.c
  * @author     baiyang
  * @date       2021-7-14
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "ErrorMode.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void SensorMode_Init(ErrorMode* pSensor, Vector3f_t State,  Vector3f_t* scale, Vector3f_t* base, Vector3f_t* variance)
{
    lpf_set_cutoff1_vec3f(&(pSensor->filter), 50.0f);    //50HZ
    lpf_reset_vec3f(&(pSensor->filter), &State);

    pSensor->_scale = *scale;
    pSensor->_base = *base;
    pSensor->_variance = *variance;

    pSensor->enable_err = true;
}

void SensorMode_Update(ErrorMode* pSensor, Vector3f_t* pState_out, const Vector3f_t* pState_in, float dt)
{
    Vector3f_t state_err;
    vec3_zero(&state_err);

    if (pSensor->enable_err)
    {
        //传感器偏差模型
        vec3_hadamard_product(&state_err, pState_in, &(pSensor->_scale));
        vec3_add(&state_err, &state_err, &(pSensor->_base));

        //滤波器
        Vector3f_t filt_out = lpf_apply2_vec3f(&(pSensor->filter), &state_err, dt);

        //加上高斯白噪声
        Vector3f_t noise;
        vec3_set(&noise, math_rand_normal(0, pSensor->_variance.x), math_rand_normal(0, pSensor->_variance.y), math_rand_normal(0, pSensor->_variance.z));
        vec3_add(pState_out, &filt_out, &noise);
    }
    else
    {
        *pState_out = *pState_out;
    }
}

void SensorMode_Enable(ErrorMode* pSensor)
{
    if (!pSensor->enable_err)
    {
        pSensor->enable_err = true;
    }
    return;
}

void SensorMode_Disable(ErrorMode* pSensor)
{
    if (pSensor->enable_err)
    {
        pSensor->enable_err = false;
    }
    return;
}

/*------------------------------------test------------------------------------*/


